/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package FRC2856.Robot;


import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Jaguar;
//import edu.wpi.first.wpilibj.camera.AxisCamera;

/**This is the motor crud
 *
 * @author asottile
 */


public class Aiming {
   
    protected ProcessImage pro;
    DriveTrain driver;
    Shooter shooter;
    
    public final static int DOWN = 0, LEFT = 1, RIGHT = 2, UP = 3; //static ints for the board to aim at
    
    //dimensions of parts of the robot in feet
    public static final double CAMERA_HEIGHT = 3.467; //height of the camera, needs to be measured for sure
    public static final double CAMERA_AHEAD = 1; //distance from axis of turning to camera, needs to be measured
    public static final double SHOOTER_HEIGHT = 2.333333333; //height of the shooter, needs to be measured for sure
    public final static double SHOOTER_AHEAD = 0.916666667; //distance from axis of turning to shooter, needs to be measured for sure
    
    //dimensions of the baskets in the Arena in feet
    public static final double BACKBOARD_WIDTH = 2, BACKBOARD_HEIGHT = 1.5; //height an width of the outside of the retroreflective square
    public static final double MID_DISPLACEMENT = 2.28125; //displacemnt from the center of the walls of the 2 - point hoops
    public static final double[] HEIGHT = {3.25, 6, 6.167, 9.083333}; //height of the backboard centers; the index referes to which backet it is
    public static final double HOOP_RADIUS = 9; //radius of the hoop
    public static final double HOOP_DISPLACEMENT = 6; //distance from hoop to backboard
    
    public Aiming(Shooter f, DriveTrain d, Jaguar j, Gyro g){
        driver = d;
        shooter = f;
        pro= new ProcessImage();
    }
    
    public Aiming(Shooter f, DriveTrain d){
        driver = d;
        shooter = f;
        pro = new ProcessImage();
    }
    
    /**
     * Preconditions: gimmee an int
     * That int will correspond to a hoop to target- use static variables
     * Postconditions: it will request the drive train to rotate and the ball speed to change and
     * have the aiming motor(s) rotate to prepare the robot to fire from its current position
     */
    public void aim(int desired){
       //double[] parameters = pro.getParameters(desired, this); //get the parameters
        double[] parameters = pro.getParameters(desired, this);
        
        //make the request to the shooting mechanism to change the speed
        //shooter.spinRateUpdate(ballSpeedToShooter(parameters[0]);

        //make the request to the drivetrain to rotate
        //driver.rotate(parameters[1]);
        double angle = driver.getGyro();
        for(int i = 0; Math.abs((driver.getGyro()-angle)-parameters[1])>1 || i < 5; i++){
            //turn the robot
        }
    }
    
    public double ballSpeedToShooter(double ballSpeed){
        return ballSpeed;
    }
}